#include "stdafx.h"

#include "gp_Ax2.h"
#include "gp_Ax1.h"
#include "gp_Mat.h"
#include "gp_Trsf.h"
#include "gp_Mat.h"
#include "gp_XYZ.h"
#include "gp_Ax1.h"
#include "gp_Ax2.h"
#include "gp_GTrsf.h"
#include "gp.h"

void gp_GTrsf::SetTranslationPart (const gp_XYZ& Coord)
{
	loc = Coord;
	if (Form() == gp_CompoundTrsf || Form() == gp_Other || 
		Form() == gp_Translation)   { }
	else if (Form() == gp_Identity) { shape = gp_Translation; }
	else                            { shape = gp_CompoundTrsf; }
}

void gp_GTrsf::Invert ()
{
	if (shape == gp_Other) {
		matrix.Invert() ;
		loc.Multiply (matrix);
		loc.Reverse();
	}
	else {
		gp_Trsf T = Trsf();
		T.Invert ();
		SetTrsf (T);       
	}
}

void gp_GTrsf::Multiply (const gp_GTrsf& T)
{
	if (Form() == gp_Other || T.Form() == gp_Other) {
		shape = gp_Other;
		loc.Add (T.loc.Multiplied (matrix));
		matrix.Multiply(T.matrix);
	}
	else {
		gp_Trsf T1 = Trsf();
		gp_Trsf T2 = T.Trsf();
		T1.Multiply(T2);
		matrix = T1.matrix;
		loc = T1.loc;
		scale = T1.scale;
		shape = T1.shape;
	}
}

void gp_GTrsf::Power (const int N)
{
	if (N == 0)  {
		scale = 1.;
		shape = gp_Identity;
		matrix.SetIdentity();
		loc = gp_XYZ (0.,0.,0.);
	}
	else if (N == 1) { }
	else if (N == -1) { Invert(); }
	else {
		if (shape == gp_Other) {
			int Npower = N;
			if (Npower < 0) Npower = - Npower;
			Npower--;
			gp_XYZ Temploc = loc;
			//      double Tempscale = scale;
			gp_Mat Tempmatrix (matrix);
			for(;;) {
				if (IsOdd(Npower)) {
					loc.Add (Temploc.Multiplied (matrix));
					matrix.Multiply (Tempmatrix);
				}
				if (Npower == 1) { break; }
				Temploc.Add (Temploc.Multiplied (Tempmatrix));
				Tempmatrix.Multiply (Tempmatrix);
				Npower = Npower/2;
			}
		}
		else {
			gp_Trsf T = Trsf ();
			T.Power (N);
			SetTrsf (T);
		}
	}
}

void gp_GTrsf::PreMultiply (const gp_GTrsf& T)
{
	if (Form() == gp_Other || T.Form() == gp_Other) {
		shape = gp_Other;
		loc.Multiply (T.matrix);
		loc.Add (T.loc);
		matrix.PreMultiply(T.matrix);
	}
	else {
		gp_Trsf T1 = Trsf();
		gp_Trsf T2 = T.Trsf();
		T1.PreMultiply(T2);
		matrix = T1.matrix;
		loc = T1.loc;
		scale = T1.scale;
		shape = T1.shape;
	}
}

void gp_GTrsf::SetForm()
{
	double tol = 1.e-12; // Precision::Angular();
	//
	// don t trust the initial values !
	//
	gp_Mat M(matrix);
	double s = M.Determinant();
	double As = s;
	if (As < 0) As = - As;
	Standard_ConstructionError_Raise_if(As < gp::Resolution(), "");
	if (s > 0)
		s = pow(s,1./3.);
	else
		s = -pow(-s,1./3.);
	M.Divide(s);

	// check if the matrix is an uniform matrix
	// the transposition should be the invert.
	gp_Mat TM(M);
	TM.Transpose();
	TM.Multiply(M);
	gp_Mat anIdentity ;
	anIdentity.SetIdentity() ;
	TM.Subtract(anIdentity);
	if (shape==gp_Other) shape = gp_CompoundTrsf;

	int i, j;
	for (i=1; i<=3; i++) {
		for (j=1; j<=3; j++) {
			As = TM.Value(i,j);
			if (As < 0) As = - As;
			if (As > tol) {
				shape = gp_Other;
				return;
			}
		}
	}
}

 gp_GTrsf::gp_GTrsf ()
{
  shape = gp_Identity;
  matrix.SetScale (1.0);
  loc.SetCoord (0.0, 0.0, 0.0);
  scale = 1.0;
}

 gp_GTrsf::gp_GTrsf (const gp_Trsf& T)
{
  shape  = T.Form();
  matrix = T.matrix;
  loc    = T.TranslationPart();
  scale  = T.ScaleFactor();
}

 gp_GTrsf::gp_GTrsf (const gp_Mat& M,
			   const gp_XYZ& V) : matrix(M), loc(V)
{
  shape = gp_Other;
  scale = 0.0;
}

 void gp_GTrsf::SetAffinity (const gp_Ax1& A1,
				   const double Ratio)
{
  shape = gp_Other;
  scale = 0.0;
  matrix.SetDot (A1.Direction().XYZ());
  matrix.Multiply (1.0 - Ratio);
  matrix.SetDiagonal (matrix.Value (1,1) + Ratio,
                      matrix.Value (2,2) + Ratio,
                      matrix.Value (3,3) + Ratio);
  loc = A1.Location().XYZ();
  loc.Reverse ();
  loc.Multiply (matrix);
  loc.Add (A1.Location().XYZ());
}

//=======================================================================
//function : SetAffinity
//history  : AGV 2-05-06: Correct the implementation
//=======================================================================
 void gp_GTrsf::SetAffinity (const gp_Ax2& A2,
				   const double Ratio)
{
  shape = gp_Other;
  scale = 0.0;
  matrix.SetDot (A2.Direction().XYZ());
  matrix.Multiply (Ratio - 1.);
  loc = A2.Location().XYZ();
  loc.Reverse ();
  loc.Multiply (matrix);
  matrix.SetDiagonal (matrix.Value (1,1) +1.,
                      matrix.Value (2,2) +1.,
                      matrix.Value (3,3) +1.);
}

 void gp_GTrsf::SetValue (const int Row,
				const int Col,
				const double Value)
{
  Standard_OutOfRange_Raise_if
    (Row < 1 || Row > 3 || Col < 1 || Col > 4, " ");
  if (Col == 4) {
    loc.SetCoord (Row, Value);
    if (shape == gp_Identity) shape = gp_Translation;
    return;
  }
  else { 
    if (!(shape == gp_Other) && !(scale == 1.0)) matrix.Multiply (scale);
    matrix.SetValue (Row, Col, Value);
    shape = gp_Other;
    scale = 0.0;
    return;
  }
}

 void gp_GTrsf::SetVectorialPart (const gp_Mat& Matrix)
{ 
  matrix = Matrix;
  shape = gp_Other;
  scale = 0.0;
}

 void gp_GTrsf::SetTrsf (const gp_Trsf& T)
{
  shape = T.shape;
  matrix = T.matrix;
  loc = T.loc;
  scale = T.scale;
}

 bool gp_GTrsf::IsNegative () const
{ return matrix.Determinant() < 0.0; }

 bool gp_GTrsf::IsSingular () const
{ return matrix.IsSingular(); }

 gp_TrsfForm gp_GTrsf::Form () const
{ return shape; }

 const gp_XYZ& gp_GTrsf::TranslationPart () const
{ return loc; }

 const gp_Mat& gp_GTrsf::VectorialPart () const
{ return matrix; }

 double gp_GTrsf::Value (const int Row,
				      const int Col) const
{
  Standard_OutOfRange_Raise_if
    (Row < 1 || Row > 3 || Col < 1 || Col > 4, " ");
  if (Col == 4) return loc.Coord (Row);
  if (shape == gp_Other) return matrix.Value (Row, Col);
  return scale * matrix.Value (Row, Col);
}

 gp_GTrsf gp_GTrsf::Inverted () const
{
  gp_GTrsf T = *this;
  T.Invert ();
  return T;
}

 gp_GTrsf gp_GTrsf::Multiplied (const gp_GTrsf& T) const
{
  gp_GTrsf Tres = *this;
  Tres.Multiply (T);
  return Tres;
}

 gp_GTrsf gp_GTrsf::Powered (const int N) const
{
  gp_GTrsf T = *this;
  T.Power (N);
  return T;
}

 void gp_GTrsf::Transforms (gp_XYZ& Coord) const
{
  Coord.Multiply (matrix);
  if (!(shape == gp_Other) && !(scale == 1.0)) Coord.Multiply (scale);
  Coord.Add(loc);
}

 void gp_GTrsf::Transforms (double& X,
				  double& Y,
				  double& Z) const
{
  gp_XYZ Triplet (X, Y, Z);
  Triplet.Multiply (matrix);
  if (!(shape == gp_Other) && !(scale == 1.0)) Triplet.Multiply (scale); 
  Triplet.Add(loc);
  Triplet.Coord (X, Y, Z);
}

 gp_Trsf gp_GTrsf::Trsf () const
{
  gp_Trsf T;
  Standard_ConstructionError_Raise_if
    (Form() == gp_Other,"");
  T.shape = shape;
  T.scale = scale;
  T.matrix = matrix;
  T.loc = loc;
  return T;
}

